#pragma once
#include "Sensor.hpp"
#include <string>
#include <AR/param.h>
#include <AR/ar.h>
#include <ooputils/OOPString.h>

class CameraSensor : public Sensor
{
private:
	class _PatternProperty
	{
	public:
		std::string thisName;
		int thisPatternId;
		int thisModelId;
		bool thisVisible;
		double thisSize;
		glm::dvec2 thisCenter;
		double thisTransMat[3][4];

		_PatternProperty (const std::string& patternName, int modelId,
				double size) : thisName (patternName), thisPatternId (-1),
				thisModelId (modelId), thisVisible (false), thisSize (size)
		{
			thisPatternId = arLoadPatt(patternName.c_str ());
			if (thisPatternId < 0)
				/* TODO: Throw something */;
		}

		void updateProperty (ARMarkerInfo* marker_info, int markerNum)
		{
			/* check for object visibility */
			thisVisible = false;
			int maxCfIndex = -1;
			for (int j = 0; j < markerNum; ++j) {
				if (thisPatternId == marker_info[j].id) {
					thisVisible = true;
					if (maxCfIndex == -1)
						maxCfIndex = j;
					else if (marker_info[maxCfIndex].cf < marker_info[j].cf)
						maxCfIndex = j;
				}
			}
			if(thisVisible) {
				arGetTransMat (&marker_info[maxCfIndex], &thisCenter[0],
						thisSize, thisTransMat);
			}
		}

		glm::dvec3 relativeLocFrom (_PatternProperty& pattern)
		{
			double wmat1[3][4], wmat2[3][4];
			arUtilMatInv(pattern.thisTransMat, wmat1);
			arUtilMatMul(wmat1, thisTransMat, wmat2);
			return glm::dvec3 (wmat2[0][3], wmat2[1][3], wmat2[2][3]);
		}

	};

	int thresh;
	glm::ivec2 frameSize;
	_PatternProperty* _basePattern;
	_PatternProperty* _fingerPattern;

public:

	CameraSensor (const OOPUtils::OOPString& configPath);

	virtual ~CameraSensor ();

	std::vector< glm::dvec3 > detectFingerPoints ();

};

